﻿// ActionScript file
//--------------------------------------------------------------
//
// Qizhi Zhang 
// ezhung@gmail.com
// http://www.moorwind.com
//
//--------------------------------------------------------------
package com.moorwind.ai.math
{
    public class Quaternion
    {
        //////////////////////////////////////////////////////////////////////////////
        //Private properties
        private var n:Number;
        private var v:Vector;
        
        //////////////////////////////////////////////////////////////////////////////
        //Public properties
        
        /** 四元数模 */
        public function get length():Number
        {
            return Math.sqrt(n*n + v.x*v.x + v.y*v.y + v.z*v.z);
        }
        
        /** 四元数向量部 */
        public function get vector():Vector
        {
            return new Vector(v.x, v.y, v.z);
        }
        
        /** 四元数纯量部 */
        public function get scalar():Number
        {
            return n;
        }
        
        /** 四元数转�?*/        
        public function get reverse():Quaternion
        {
            return new Quaternion(n, -v.x, -v.y, -v.z);
        }
        
        /** 四元数幅�?*/    
        public function get angle():Number
        {
            return 2 * Math.cos(n);
        }    
        /** 四元数幅�?*/    
        public static function getAngle(q:Quaternion):Number
        {
            return 2 * Math.cos(q.n);
        }
        
        public function get axis():Vector
        {
            var m:Number = v.length;
            
            if (m <= MathConst.TOL)
                return new Vector();
            else
                return Vector.division(v, m);    
        }    

        public static function getAxis(q:Quaternion):Vector
        {
            var v:Vector;
            var m:Number;
        
            v = q.v;
            m = v.length
            
            if (m <= MathConst.TOL)
                return new Vector();
            else
                return Vector.division(v, m);        
        }
        
        //////////////////////////////////////////////////////////////////////////////
        //Constructor
        public function Quaternion(e0:Number = 0, e1:Number = 0, e2:Number = 0, e3:Number = 0)
        {
            this.n = e0;
            this.v = new Vector(e1, e2, e3);
        }
        
        //////////////////////////////////////////////////////////////////////////////
        //methods
        
        /** 四元�?= */
        public function add(q:Quaternion):void
        {
            n += q.n;
            v.add(q.v);
        }
        
        /** 四元�?= */
        public function sub(q:Quaternion):void
        {
            n -= q.n;
            v.sub(q.v);
        }
        
        /** 四元�?= */
        public function mul(s:Number):void
        {
            n *= s;
            v = Vector.multiplication(s, v);
        }
        
        /** 四元�?= */
        public function div(s:Number):void
        {
            n /= s;
            v = Vector.division(v, s);
        }
        
        /** 四元数加�?*/
        public static function addition(q1:Quaternion, q2:Quaternion):Quaternion
        {
            return new Quaternion(    q1.n + q2.n,
                                    q1.v.x + q2.v.x,
                                    q1.v.y + q2.v.y,
                                    q1.v.z + q2.v.z);
        }
        
        /** 四元数减�?*/
        public static function subtraction(q1:Quaternion, q2:Quaternion):Quaternion
        {
            return new Quaternion(    q1.n - q2.n,
                                    q1.v.x - q2.v.x,
                                    q1.v.y - q2.v.y,
                                    q1.v.z - q2.v.z);
        }
        
        /** 四元数除�?*/
        public static function division(q:Quaternion, s:Number):Quaternion
        {
            return new Quaternion(q.n/s, q.v.x/s, q.v.y/s, q.v.z/s);
        }
        
        /** 四元数乘�?*/
        public static function multiplication(q1:Quaternion, q2:Quaternion):Quaternion
        {
            return new Quaternion(    q1.n*q2.n - q1.v.x*q2.v.x - q1.v.y*q2.v.y - q1.v.z*q2.v.z,
                                    q1.n*q2.v.x + q1.v.x*q2.n + q1.v.y*q2.v.z - q1.v.z*q2.v.y,
                                    q1.n*q2.v.y + q1.v.y*q2.n + q1.v.z*q2.v.x - q1.v.x*q2.v.z,
                                    q1.n*q2.v.z + q1.v.z*q2.n + q1.v.x*q2.v.y - q1.v.y*q2.v.x);    
        }
        
        /** 四元数乘�?*/
        public static function QSMulti(q:Quaternion, s:Number):Quaternion
        {
            return new Quaternion(q.n*s, q.v.x*s, q.v.y*s, q.v.z*s);
        }
        
        /** 四元数乘�?*/
        public static function SQMulti(s:Number, q:Quaternion):Quaternion
        {
            return new Quaternion(q.n*s, q.v.x*s, q.v.y*s, q.v.z*s);
        }
        
        /** 四元数乘�?*/
        public static function QVMulti(q:Quaternion, v:Vector):Quaternion
        {
            return new Quaternion(    -(q.v.x*v.x + q.v.y*v.y + q.v.z*v.z),
                                    q.n*v.x + q.v.y*v.z - q.v.z*v.y,
                                    q.n*v.y + q.v.z*v.x - q.v.x*v.z,
                                    q.n*v.z + q.v.x*v.y - q.v.y*v.x);
        }
        
        /** 四元数乘�?*/
        public static function VQMulti(v:Vector, q:Quaternion):Quaternion
        {
            return new Quaternion(    -(q.v.x*v.x + q.v.y*v.y + q.v.z*v.z),
                            q.n*v.x + q.v.z*v.y - q.v.y*v.z,
                            q.n*v.y + q.v.x*v.z - q.v.z*v.x,
                            q.n*v.z + q.v.y*v.x - q.v.x*v.y);
        }
        
        public static function QRotate(q1:Quaternion, q2:Quaternion):Quaternion
        {
            return Quaternion.multiplication( Quaternion.multiplication(q1,q2), q1.reverse);
        }
        
        public static function QVRotate(q:Quaternion, v:Vector):Vector
        {
            var t:Quaternion = Quaternion.multiplication(Quaternion.QVMulti(q, v), q.reverse);
            return t.v;
        }
        
        public static function makeQFromEulerAngles(x:Number, y:Number, z:Number):Quaternion
        {
            var q:Quaternion;
            var    roll:Number = MathUtils.DegreesToRadians(x);
            var    pitch:Number = MathUtils.DegreesToRadians(y);
            var    yaw:Number = MathUtils.DegreesToRadians(z);
            
            var    cyaw:Number, cpitch:Number, croll:Number, syaw:Number, spitch:Number, sroll:Number;
            var    cyawcpitch:Number, syawspitch:Number, cyawspitch:Number, syawcpitch:Number;
        
            cyaw = Math.cos(0.5 * yaw);
            cpitch = Math.cos(0.5 * pitch);
            croll = Math.cos(0.5 * roll);
            syaw = Math.sin(0.5 * yaw);
            spitch = Math.sin(0.5 * pitch);
            sroll = Math.sin(0.5 * roll);
        
            cyawcpitch = cyaw*cpitch;
            syawspitch = syaw*spitch;
            cyawspitch = cyaw*spitch;
            syawcpitch = syaw*cpitch;
        
            q.n = cyawcpitch * croll + syawspitch * sroll;
            q.v.x = cyawcpitch * sroll - syawspitch * croll; 
            q.v.y = cyawspitch * croll + syawcpitch * sroll;
            q.v.z = syawcpitch * croll - cyawspitch * sroll;
        
            return q;
        }
        
        public static function makeEulerAnglesFromQ(q:Quaternion):Vector
        {
            var    r11:Number, r21:Number, r31:Number, r32:Number, r33:Number, r12:Number, r13:Number;
            var    q00:Number, q11:Number, q22:Number, q33:Number;
            var    tmp:Number;
            var    u:Vector;
        
            q00 = q.n * q.n;
            q11 = q.v.x * q.v.x;
            q22 = q.v.y * q.v.y;
            q33 = q.v.z * q.v.z;
        
            r11 = q00 + q11 - q22 - q33;
            r21 = 2 * (q.v.x*q.v.y + q.n*q.v.z);
            r31 = 2 * (q.v.x*q.v.z - q.n*q.v.y);
            r32 = 2 * (q.v.y*q.v.z + q.n*q.v.x);
            r33 = q00 - q11 - q22 + q33;
        
            tmp = Math.abs(r31);
            if(tmp > 0.999999)
            {
                r12 = 2 * (q.v.x*q.v.y - q.n*q.v.z);
                r13 = 2 * (q.v.x*q.v.z + q.n*q.v.y);
        
                u.x = MathUtils.RadiansToDegrees(0.0); //roll
                u.y = MathUtils.RadiansToDegrees((-(MathConst.PI/2) * r31/tmp)); // pitch
                u.z = MathUtils.RadiansToDegrees(Math.atan2(-r12, -r31*r13)); // yaw
                return u;
            }
        
            u.x = MathUtils.RadiansToDegrees(Math.atan2(r32, r33)); // roll
            u.y = MathUtils.RadiansToDegrees(Math.asin(-r31));         // pitch
            u.z = MathUtils.RadiansToDegrees(Math.atan2(r21, r11)); // yaw
            return u;
        }
        
        
    }
}